/********************************************************************************
 * Copyright (c)2016-2020, ShenZhen WanJiaAn Technology CO.,LTD.
 * All Rights Reserved.
 *
 * File Name     : icmp_ping.c
 * Author        : joki.zhu
 * Date          : 2020-04-23
 *
 * Record        :
 * 1.Date        : 2020-04-23
 *   Author      : joki.zhu
 *   Modification: Created file
 ********************************************************************************/
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <signal.h>
#include <sys/select.h>
#include <arpa/inet.h>
#include <sys/types.h>
#include <sys/time.h>
#include <sys/socket.h>
#include <unistd.h>
#include <netinet/in.h>
#include <netinet/ip.h>
#include <netinet/ip_icmp.h>
#include <netdb.h>
#include <setjmp.h>
#include <errno.h>
#include <fcntl.h>

#ifdef __HuaweiLite__
#include "netif/ifaddrs.h"
#include "lwip/sockets.h"
#include "lwip/api.h"
#include "lwip/sys.h"
#include "lwip/igmp.h"
#include "lwip/inet.h"
#include "lwip/tcp.h"
#include "lwip/raw.h"
#include "lwip/udp.h"
#include "lwip/tcpip.h"
#include "lwip/pbuf.h"
#include "lwip/netif.h"
#include "lwip/ip_addr.h"
#include "lwip/dhcp.h"
#include "lwip/filter.h"
#endif /* __HuaweiLite__ */

//#define ICMP_PING_DEBUG

#define ICMP_HEAD_LEN	(8)
#define ICMP_DATA_LEN	(56)
#define ICMP_PKT_SIZE	(256)

#define ICMP_RECV_NONBLOCK_MODE /* 非阻塞接收使能开关 */

static pid_t gs_icmpPid;
static char gs_icmpSendBuf[ICMP_PKT_SIZE];
static char gs_icmpRecvBuf[ICMP_PKT_SIZE];

/* 校验和算法 */
static unsigned short getChksum(unsigned short *addr,int len)
{   
    int nleft = len;
    int sum=0;
    unsigned short *w = addr;
    unsigned short answer = 0;

    /* 把ICMP报头二进制数据以2字节为单位累加起来 */
    while(nleft>1)
    {   
        sum += *w++;
        nleft -= 2;
    }
    /* 若ICMP报头为奇数个字节，会剩下最后一字节。把最后一个字节视为一个2字节数据的高字节，这个2字节数据的低字节为0，继续累加 */
    if(nleft==1)
    {   
        *(unsigned char *)(&answer) = *(unsigned char *)w;
        sum += answer;
    }
    sum = (sum>>16)+(sum&0xffff);
    sum += (sum>>16);
    answer = ~sum;
    return answer;
}

/* 设置ICMP报头 */
static int packIcmp(int pack_no, struct icmp* icmp)
{   
    int packsize;
    struct icmp *picmp;
    struct timeval *tval;

    picmp = icmp;
    picmp->icmp_type = ICMP_ECHO;
    picmp->icmp_code = 0;
    picmp->icmp_cksum = 0;
    picmp->icmp_seq = pack_no;
    picmp->icmp_id = gs_icmpPid;
    packsize = ICMP_HEAD_LEN + ICMP_DATA_LEN;
    tval = (struct timeval *)icmp->icmp_data;
    gettimeofday(tval, NULL);    /* 记录发送时间 */
    picmp->icmp_cksum = getChksum((unsigned short *)icmp,packsize); /* 校验算法 */
    return packsize;
}

/* 两个timeval结构相减 */
static struct timeval tvSub(struct timeval timeval1,struct timeval timeval2)
{ 	
    struct timeval result;
    result = timeval1;
    if (result.tv_usec < timeval2.tv_usec && timeval2.tv_usec < 0)
    {    
        --result.tv_sec;
        result.tv_usec += 1000000;
    }
    result.tv_sec -= timeval2.tv_sec;
    return result;
}

/* 剥去ICMP报头 */
static int unpackIcmp(char *buf,int len)
{   
    int iphdrlen;
    struct ip *ip;
    struct icmp *icmp;
    struct timeval *tvsend, tvrecv, tvresult;
    double rtt;

    ip = (struct ip *)buf;
    iphdrlen = ip->ip_hl << 2;    /* 求ip报头长度,即ip报头的长度标志乘4 */
    icmp = (struct icmp *)(buf + iphdrlen);  /*越过ip报头,指向ICMP报头 */
    len -= iphdrlen;            /* ICMP报头及ICMP数据报的总长度 */
    if(len < 8)                /* 小于ICMP报头长度则不合理 */
    {   
        printf("ICMP packets\'s length is less than 8\n");
        return -1;
    }
    /* 确保所接收的是我所发的的ICMP的回应 */	
    if( (icmp->icmp_type==ICMP_ECHOREPLY) && (icmp->icmp_id == gs_icmpPid) )
    {   
        tvsend = (struct timeval *)icmp->icmp_data;
        gettimeofday(&tvrecv,NULL);  /* 记录接收时间 */
        tvresult = tvSub(tvrecv, *tvsend);  /* 接收和发送的时间差 */
        rtt = tvresult.tv_sec*1000 + tvresult.tv_usec/1000;  /* 以毫秒为单位计算rtt */
        return 0;
    }
    else {
        return -1;
    }
}

/* 发送ICMP报文 */
static int sendIcmpPacket(int sockfd, struct sockaddr_in *dest_addr, char *sendBuf, int sendCnt)
{
    size_t packetsize = 0;

    packetsize = packIcmp(sendCnt, (struct icmp*)sendBuf); /* 设置ICMP报头 */
    if(sendto(sockfd, sendBuf, packetsize, 0, (struct sockaddr *)dest_addr, sizeof(struct sockaddr_in)) < 0){   
#ifdef ICMP_PING_DEBUG
        perror("sendto error");
#endif /* ICMP_PING_DEBUG */
        return -1;
    }

    return 0;
}

/* 接收ICMP报文 */
static int recvIcmpPacket(int sockfd, struct sockaddr_in *src_addr, char *recvBuf)
{
    int recvLen = 0;
    int addrLen = sizeof(struct sockaddr_in);
    int retryCnt = 0;

retry_again:	
    if((recvLen = recvfrom(sockfd, recvBuf, ICMP_PKT_SIZE, 0, (struct sockaddr *)src_addr, (socklen_t *)&addrLen)) <0) {
        printf("recvfrom failed!\n");
        return -1;
    }

    if(unpackIcmp(recvBuf, recvLen) == -1) 
    {
        printf("unpackIcmp failed! recvLen:%d\n", recvLen);
        if (++retryCnt >= 2) {
            return -1;
        }
        goto retry_again;
    }
	
    return 0;
}

static int domain_to_ip(char *inAddr, char *outIp)
{
    struct addrinfo * res;
    struct addrinfo hints;
    int succ = 0;
    struct sockaddr_in dst_addr;

    /* Judge whether the input address is domain name or IP */
    memset(&dst_addr, 0, sizeof(dst_addr));
    if ((dst_addr.sin_addr.s_addr = inet_addr(inAddr)) != INADDR_NONE)  {
        sprintf(outIp, "%s", inAddr);
        return 0;
    }
	
    memset(&hints, 0, sizeof(struct addrinfo));
    hints.ai_family = AF_INET; /* Allow IPv4 */
    hints.ai_flags = AI_PASSIVE; /* For wildcard IP address */
    hints.ai_protocol = 0; /* Any protocol */
    hints.ai_socktype = SOCK_STREAM;
    succ = getaddrinfo(inAddr, NULL, &hints, &res);
    if (succ != 0) {
#ifdef ICMP_PING_DEBUG
        printf("can't get address info! error code = %d\n", succ);
#endif /* ICMP_PING_DEBUG */
        return -1;
	} else {
        if (res->ai_addr->sa_family == AF_INET)
        {
            if (outIp != NULL) {
                struct sockaddr_in* pServAddr = (struct sockaddr_in *)res->ai_addr;
                inet_ntop(AF_INET, &(pServAddr->sin_addr), outIp, INET_ADDRSTRLEN);
            }
        }
    }

    freeaddrinfo(res);

    return 0;
}

static int createIcmpSocket(char *hostname, struct sockaddr_in *out_dst_addr)
{
    int sockfd = -1;
    struct sockaddr_in dst_addr;
    char ip[16] = "";
	
    memset(&dst_addr, 0, sizeof(dst_addr));
    /* 判断主机名不是点分十进制的IP,则采用域名解析 */
    if ((dst_addr.sin_addr.s_addr = inet_addr(hostname)) == INADDR_NONE) 
    {
        if (domain_to_ip(hostname, ip) < 0) {
#ifdef ICMP_PING_DEBUG
            printf("domain_to_ip %s failed\n", hostname);
#endif /* ICMP_PING_DEBUG */
            return -1;
        }
        if (!inet_aton(ip, &(dst_addr.sin_addr))) {
            printf("inet_aton ip %s failed\n", ip);
            return -1;
        }
    } else if (!inet_aton(hostname, &dst_addr.sin_addr)){
        printf("inet_aton hostname %s failed\n", hostname);
        return -1;
    }
    dst_addr.sin_family = AF_INET;

    if ((sockfd = socket(AF_INET, SOCK_RAW, IPPROTO_ICMP)) < 0) {
        perror("socket error");
        return -1;
    }

    *out_dst_addr = dst_addr;

    return sockfd;
}

int icmp_ping(char *hostname, int count, int timeoutMs)
{
    int i = 0;
    int ret = 0;
    int sockfd = -1;
    struct sockaddr_in src_addr, dst_addr;
    int rcvCnt = 0;
    char bInternetOk = 0;

    memset(&gs_icmpSendBuf, 0, sizeof(gs_icmpSendBuf));
    memset(&gs_icmpRecvBuf, 0, sizeof(gs_icmpRecvBuf));

    memset(&src_addr, 0, sizeof(src_addr));
    memset(&dst_addr, 0, sizeof(dst_addr));

    sockfd = createIcmpSocket(hostname, &dst_addr);
    if (sockfd < 0) {
#ifdef ICMP_PING_DEBUG
        printf("createIcmpSocket %s failed!\n", hostname);
#endif /* ICMP_PING_DEBUG */
        goto fail_exit;
    } 

    gs_icmpPid = getpid();
	
#ifdef ICMP_RECV_NONBLOCK_MODE
    fd_set readfds;
    struct timeval tvTimeout;

    int sockfdops = fcntl(sockfd, F_GETFL);
    if (sockfdops < 0) {
        printf("fcntl F_GETFL failed\n");
        goto close_fd;
    }
    if (fcntl(sockfd, F_SETFL, sockfdops | O_NONBLOCK) < 0) {
        printf("fcntl F_SETFL failed\n");
        goto close_fd;
    }

    FD_ZERO(&readfds);
    FD_SET(sockfd, &readfds);
#endif /* ICMP_RECV_BLOCK_MODE */	
	
    while (i < count)
    {
        ret = sendIcmpPacket(sockfd, &dst_addr, gs_icmpSendBuf, i);
        if (ret < 0) {
            usleep(10 * 1000);
            continue;
        }
#ifdef ICMP_RECV_NONBLOCK_MODE		
        tvTimeout.tv_sec = timeoutMs/1000;
        tvTimeout.tv_usec = timeoutMs%1000 * 1000;

        ret = select(sockfd + 1, &readfds, NULL, NULL, &tvTimeout);
        if (ret <= 0) {
            i++;
            printf("ping %s select timeout\n", hostname);
            continue;
        }
		
        if (FD_ISSET(sockfd, &readfds)) {
            ret = recvIcmpPacket(sockfd, &src_addr, gs_icmpRecvBuf);
            if (ret >= 0) {
                rcvCnt++;
            }			
        }
#else
        usleep(100 * 1000);
        ret = recvIcmpPacket(sockfd, &src_addr, gs_icmpRecvBuf);
        if (ret >= 0) {
            rcvCnt++;
        }	
#endif /* ICMP_RECV_BLOCK_MODE */			

        i++;
    }

    if (rcvCnt > 0) {
        bInternetOk = 1;
    } 
	
    printf("ping %s sendCnt:%d rcvCnt:%d bInternetOk:%d\n", hostname, count, rcvCnt, bInternetOk);

close_fd:
	close(sockfd);
fail_exit:
    return bInternetOk;
}

#if 0
int main(int argc, const char *argv[])
{
    if (argc < 3) {
        printf("Usage: %s [addr] [count]\n", argv[0]);
        return -1;
    }
    
	char bInternetOk = 0;

    bInternetOk = icmp_ping((char *)argv[1], atoi(argv[2]), 500);
    if (bInternetOk) {
        printf("internet ok!\n");
    }

    return 0;
}
#endif
